/**
 * Arduino两轮小车
 * 
 * 本程序可不受限制的用于学习和教学，商业用途请联系作者。
 * 本程序不对用户因使用程序而造成任何形式的损失负责。
 * 
 * site: http://www.xpstem.com/maker/arduino-vehicle
 * author: billy zhang
 */
 
#include <Arduino.h>
#include "UltrasonicLidar.h"

#define LookDelay 100//转动舵机的停顿时间

UltrasonicLidar::UltrasonicLidar(uint8_t echoPin, uint8_t trigPin, uint8_t servoPin)
  : echoPin_(echoPin), trigPin_(trigPin), servoPin_(servoPin)
{
  pinMode(echoPin_, INPUT);
  pinMode(trigPin_, OUTPUT); 
  pinMode(servoPin_, OUTPUT);
  servo_.attach(servoPin_);
}

float UltrasonicLidar::scan(uint8_t angle)
{
  servo_.write(angle);
  delay(LookDelay);
  
  float distance;
  digitalWrite(trigPin_, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin_, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin_, LOW);
  // 检测脉冲宽度，并计算出距离
  distance = pulseIn(echoPin_, HIGH) / 58.00;

  Serial.print("Distance:");
  Serial.print(distance);
  Serial.print("cm");
  Serial.println();
  
  return distance;
}

UltrasonicLidar::~UltrasonicLidar()
{
}
